#include "WPILib.h"

/**
 * This is a demo program showing the use of the 	RobotBase class.
 * The SimpleRobot class is the base of a robot application that will automatically call your
 * Autonomous and OperatorControl methods at the right time as controlled by the switches on
 * the driver station or the field controls.
 */
class DefaultRobot : public SimpleRobot
{
	RobotDrive *myRobot;			// robot drive system
	DigitalInput *armUpperLimit;	// arm upper limit switch
	DigitalInput *armLowerLimit;	// arm lower limit switch
	Joystick *rightStick;			// joystick 1 (arcade stick or right tank stick)
	Joystick *leftStick;			// joystick 2 (tank left stick)
	Joystick *armStick;				// joystick 3 to control arm
	DriverStation *ds;				// driver station object
	SpeedController *armLeftMotor;
	SpeedController *armRightMotor;
	Compressor		*compressor;
	Relay			*grabber;
	AnalogChannel	*freePot;
	Encoder			*rightEncoder;
	Encoder			*leftEncoder;
	
	

	enum							// Driver Station jumpers to control program operation
	{ ARCADE_MODE = 1,				// Tank/Arcade jumper is on DS Input 1 (Jumper present is arcade)
	  ENABLE_AUTONOMOUS = 2,		// Autonomous/Teleop jumper is on DS Input 2 (Jumper present is autonomous)
	} jumpers;	                            

public:
	/**
	 * 
	 * 
	 * Constructor for this robot subclass.
	 * Create an instance of a RobotDrive with left and right motors plugged into PWM
	 * ports 1 and 2 on the first digital module.
	 */
	DefaultRobot(void)
	{
		ds = DriverStation::GetInstance();
		myRobot       = new RobotDrive(1, 2);	// create robot drive base
		rightStick    = new Joystick(1);			// create the joysticks
		leftStick     = new Joystick(2);
		armStick      = new Joystick(3);
		armLeftMotor  = new Victor(3);
		armRightMotor = new Victor(4);
		compressor    = new Compressor(14, 2);
		compressor->Start();
		grabber		  = new Relay(1);
		freePot		  = new AnalogChannel(1, 7);
		rightEncoder  = new Encoder(1,3,true);
		leftEncoder	  = new Encoder(2,4,true);
		//armUpperLimit = new DigitalInput(1);	// create the limit switch inputs
		//armLowerLimit = new DigitalInput(2);
		//Update the motors at least every 100ms.
		GetWatchdog().SetExpiration(100);
	}

	/**
	 * Drive left & right motors for 2 seconds, enabled by a jumper (jumper
	 * must be in for autonomous to operate).
	 */
	void Autonomous(void)
	{
		GetWatchdog().SetEnabled(false);
		if (ds->GetDigitalIn(ENABLE_AUTONOMOUS) == 1)	// only run the autonomous program if jumper is in place
		{
			myRobot->Drive(0.5, 0.0);			// drive forwards half speed
			Wait(2000);							//    for 2 seconds
			myRobot->Drive(0.0, 0.0);			// stop robot
		}
		GetWatchdog().SetEnabled(true);
	}

	/**
	 * Runs the motors under driver control with either tank or arcade steering selected
	 * by a jumper in DS Digin 0. Also an arm will operate based on a joystick Y-axis. 
	 */
	void OperatorControl(void)
	{
		float an7Voltage;
		float an7MeanVoltage;
		signed short an7Value;
		signed int an7MeanValue; 
		while (1)
		{
			GetWatchdog().Feed();
			
			an7Voltage = freePot->GetVoltage();
			an7MeanVoltage = freePot->GetAverageVoltage();
			an7Value = freePot->GetValue();
			an7MeanValue = freePot->GetAverageValue();
			printf("Analog input 7 volt:%f Mvolt:%f val:%i Mval:%i\n", an7Voltage, an7MeanVoltage, an7Value, an7MeanValue);
			
			// determine if tank or arcade mode; default with no jumper is for tank drive	
			myRobot->TankDrive(leftStick, rightStick);	 // drive with tank style

			// Control the movement of the arm using the joystick
			// Use the "Y" value of the arm joystick to control the movement of the arm
			float armStickDirection = armStick->GetY();

			// if at a limit and telling the arm to move past the limit, don't drive the motor
#ifdef NOTDEF
			if ((armUpperLimit->Get() == 0) && (armStickDirection > 0.0)) {
				armStickDirection = 0;
			} else if ((armLowerLimit->Get() == 0) && (armStickDirection < 0.0)) {
				armStickDirection = 0;
			}
#endif

			// Set the motor value 
			armRightMotor->Set(armStickDirection);
			if(armStickDirection > 0)
				armLeftMotor->Set(armStickDirection * -1.0f);
			else
				armLeftMotor->Set(0.0f);
			
			if(DriverStation::GetInstance()->GetDigitalIn(2))
				grabber->Set(Relay::kForward);
			else
				grabber->Set(Relay::kOff);
		}
	}
};

START_ROBOT_CLASS(DefaultRobot);


